#include <rtthread.h>
#include <rtdevice.h>
#include <stdlib.h>
#include <rtdbg.h>

/* 电机引脚定义 */
#define MOT1_A    22    // 左电机方向A
#define MOT1_B    23    // 左电机方向B
#define MOT1_PWM  21    // 左电机PWM
#define MOT2_A    15    // 右电机方向A
#define MOT2_B    12    // 右电机方向B
#define MOT2_PWM  11    // 右电机PWM

/* PWM参数 */
#define PWM_COUNT_MAX  11    // 计数上限（0~9）
#define PWM_TIMER_US    3    // 定时器周期5μs → 5×10=50μs → 20kHz

/* 全局变量：左右电机占空比（核心控制变量） */
static rt_timer_t pwm_timer1 = RT_NULL;  // 左电机PWM
static rt_timer_t pwm_timer2 = RT_NULL;  // 右电机PWM
static int left_duty  = 0;  // 左电机占空比（0~20）
static int right_duty = 0;  // 右电机占空比（0~20）

/* 左电机PWM回调 */
static void pwm_left_callback(void *parameter)
{
    static uint8_t count = 0;
    rt_pin_write(MOT1_PWM, (count < left_duty) ? PIN_HIGH : PIN_LOW);
    count = (count + 1) % PWM_COUNT_MAX;
}

/* 右电机PWM回调 */
static void pwm_right_callback(void *parameter)
{
    static uint8_t count = 0;
    rt_pin_write(MOT2_PWM, (count < right_duty) ? PIN_HIGH : PIN_LOW);
    count = (count + 1) % PWM_COUNT_MAX;
}

/* 限制占空比范围（替代rt_clamp，解决编译错误） */
static int limit_duty(int duty)
{
    if (duty < 0) return 0;
    if (duty > PWM_COUNT_MAX) return PWM_COUNT_MAX;
    return duty;
}

/* 设置左电机占空比 */
void set_left_duty(int duty)
{
    left_duty = limit_duty(duty);
}

/* 设置右电机占空比 */
void set_right_duty(int duty)
{
    right_duty = limit_duty(duty);
}

/* 前进（双电机同速） */
void motor_forward(int duty)
{
    duty = limit_duty(duty);
    set_left_duty(duty);
    set_right_duty(duty);
    // 前进方向：A高B低
    rt_pin_write(MOT1_A, PIN_HIGH);
    rt_pin_write(MOT1_B, PIN_LOW);
    rt_pin_write(MOT2_A, PIN_HIGH);
    rt_pin_write(MOT2_B, PIN_LOW);
}

/* 前进（双电机同速） */
void motor_back(int duty)
{
    duty = limit_duty(duty);
    set_left_duty(duty);
    set_right_duty(duty);
    // 前进方向：A高B低
    rt_pin_write(MOT1_A, PIN_HIGH);
    rt_pin_write(MOT1_B, PIN_HIGH);
    rt_pin_write(MOT2_A, PIN_HIGH);
    rt_pin_write(MOT2_B, PIN_HIGH);
    rt_thread_mdelay(10);

    rt_pin_write(MOT1_A, PIN_LOW);
    rt_pin_write(MOT1_B, PIN_HIGH);
    //目前怀疑a15引脚有问题  拉不低 只能下拉到2.几伏 pa15是特殊引脚 需要特殊处理
    rt_pin_write(MOT2_A, PIN_LOW);
    rt_pin_write(MOT2_B, PIN_HIGH);
}

/* 左转（左慢右快） */
void motor_turn_left(int slow_duty, int fast_duty)
{
    set_left_duty(limit_duty(slow_duty));
    set_right_duty(limit_duty(fast_duty));
    // 方向不变（前进）
    rt_pin_write(MOT1_A, PIN_HIGH);
    rt_pin_write(MOT1_B, PIN_LOW);
    rt_pin_write(MOT2_A, PIN_HIGH);
    rt_pin_write(MOT2_B, PIN_LOW);
}

/* 右转（右慢左快） */
void motor_turn_right(int slow_duty, int fast_duty)
{
    set_left_duty(limit_duty(fast_duty));
    set_right_duty(limit_duty(slow_duty));
    // 方向不变（前进）
    rt_pin_write(MOT1_A, PIN_HIGH);
    rt_pin_write(MOT1_B, PIN_LOW);
    rt_pin_write(MOT2_A, PIN_HIGH);
    rt_pin_write(MOT2_B, PIN_LOW);
}

/* 停止（双电机占空比0） */
void motor_stop(void)
{
    set_left_duty(0);
    set_right_duty(0);
}

/* 电机初始化线程 */
static void motor_init_thread(void *parameter)
{
    /* 初始化引脚 */
    rt_pin_mode(MOT1_A, PIN_MODE_OUTPUT);
    rt_pin_mode(MOT1_B, PIN_MODE_OUTPUT);
    rt_pin_mode(MOT1_PWM, PIN_MODE_OUTPUT);
    rt_pin_mode(MOT2_A, PIN_MODE_OUTPUT);
    rt_pin_mode(MOT2_B, PIN_MODE_OUTPUT);
    rt_pin_mode(MOT2_PWM, PIN_MODE_OUTPUT);

    /* 创建PWM定时器 */
    pwm_timer1 = rt_timer_create("pwm_left", pwm_left_callback, RT_NULL, PWM_TIMER_US, RT_TIMER_FLAG_PERIODIC);
    pwm_timer2 = rt_timer_create("pwm_right", pwm_right_callback, RT_NULL, PWM_TIMER_US, RT_TIMER_FLAG_PERIODIC);

    /* 启动定时器 */
    if (pwm_timer1) rt_timer_start(pwm_timer1);
    if (pwm_timer2) rt_timer_start(pwm_timer2);

    /* 初始状态：停止 */
    motor_stop();

    while (1) rt_thread_mdelay(1000);
}

/* 启动电机控制 */
int motor_start(void)
{
    rt_thread_t tid = rt_thread_create("motor_init", motor_init_thread, RT_NULL, 1024, 8, 20);
    if (tid) rt_thread_startup(tid);
    return 0;
}
INIT_APP_EXPORT(motor_start);

/* 兼容旧代码调用（解决undefined reference错误） */
void stop_motors_pwm(void) { motor_stop(); }
void set_motors_speed(int spd) { motor_forward(spd); }
